#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <mutex>
 
cv::Mat image;
std::mutex image_mutex;

void imageCallback(const sensor_msgs::ImageConstPtr &msg)
{
  ROS_INFO_STREAM("Get Depth Msg");
  try
  {
    cv::Mat depth_image = cv_bridge::toCvShare(msg, "16UC1")->image;
    
    // 🔥 关键修复：深度图像归一化
    cv::Mat depth_normalized;
    double min_val, max_val;
    cv::minMaxLoc(depth_image, &min_val, &max_val);
    
    ROS_INFO_STREAM("Depth range: " << min_val << " - " << max_val);
    
    if (max_val > 0) {
      cv::normalize(depth_image, depth_normalized, 0, 255, cv::NORM_MINMAX, CV_8UC1);
    } else {
      depth_normalized = cv::Mat::zeros(depth_image.size(), CV_8UC1);
    }
    
    // 线程安全显示
    std::lock_guard<std::mutex> lock(image_mutex);
    cv::imshow("view", depth_normalized);
  }
  catch (cv_bridge::Exception &e)
  {
    ROS_ERROR("Could not convert from '%s' to '16UC1'.", msg->encoding.c_str());
  }
  catch (cv::Exception &e)
  {
    ROS_ERROR("OpenCV error: %s", e.what());
  }
}

void imageCallback2(const sensor_msgs::ImageConstPtr &msg)
{
  ROS_INFO_STREAM("Get Color Msg");
  try
  {
    cv::Mat color_image = cv_bridge::toCvShare(msg, "bgr8")->image;
    
    // 线程安全显示
    std::lock_guard<std::mutex> lock(image_mutex);
    cv::imshow("view2", color_image);
  }
  catch (cv_bridge::Exception &e)
  {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
  }
  catch (cv::Exception &e)
  {
    ROS_ERROR("OpenCV error: %s", e.what());
  }
}
 
int main(int argc, char **argv)
{
  ros::init(argc, argv, "image_listener");
  ros::NodeHandle nh;
  
  // 设置OpenCV线程数
  cv::setNumThreads(1);
  
  cv::namedWindow("view", cv::WINDOW_AUTOSIZE);
  cv::namedWindow("view2", cv::WINDOW_AUTOSIZE);
  
  image_transport::ImageTransport it(nh);
  image_transport::Subscriber sub = it.subscribe("/camera/depth/image_rect_raw", 1, imageCallback);
  image_transport::Subscriber sub2 = it.subscribe("/camera/color/image_raw", 1, imageCallback2);
  
  ROS_INFO("Image display node started. Press Ctrl+C to exit.");
  
  // 主循环
  ros::Rate rate(30);  // 30Hz
  while (ros::ok()) {
    ros::spinOnce();
    
    // 安全的waitKey调用
    int key = cv::waitKey(1) & 0xFF;
    if (key == 27) {  // ESC键退出
      break;
    }
    
    rate.sleep();
  }
  
  cv::destroyAllWindows();
  ROS_INFO("Image display node stopped.");
  return 0;
}

